float sinus (float degree) {return (float)sin(6.28*(float)degree/360);}
float cosinus (float degree) {return (float)cos(6.28*(float)degree/360);}
float rad2deg (float radian) {
  while (radian<0) radian+=6.28;
  while (radian>6.28) radian-=6.28;
  return 360*(radian/6.28);
}
float absD(float x) {
  if (x>=0) return x;
  return -x;
}
/***********************************************************/
/** B A S I C  V E C T O R  &  M A T R I X  F U N C T I O N S **/
/***************************************************************/
void DogaPrintMatrixCol(String matrixName,float[][] Matrix,int row,int col/*PRINTS IN COL ORDER*/) {
  println("Printing the matrix = "+matrixName+" in coloumn based order");
  for (int i=0;i<row;i++) {
	for (int j=0;j<col;j++) {
		print("["+Matrix[i+j*row]+"],");
	}
	println();
  }
  println();
  return;
}
void DogaPrintMatrixRow(String matrixName,float[][] Matrix,int row,int col/*PRINTS IN COL ORDER*/) {
  println("Printing the matrix = "+matrixName+" in row based order");
  for (int i=0;i<row;i++) {
	for (int j=0;j<col;j++) {
		print("["+Matrix[j+i*col]+"],");
	}
	println();
  }
  println();
  return;
}
void DogaCopyMatrix(float[] from,float[] to,int n){
  int i=0;
  for (i=0;i<n;i++) to[i]=from[i];
  return;
}
void DogaCopyMatrix(float[][] from,float[][] to,int row,int col){
  int i=0,j=0;
  for (i=0;i<row;i++)
    for(j=0;j<col;j++)
      to[i][j]=from[i][j];
  return;
}
float DogaDotProduct(float V1[],float V2[]) {return V1[0]*V2[0]+V1[1]*V2[1]+V1[2]*V2[2];}
float[] DogaCrossProduct(float V1[],float V2[]) {
  float m;
  float[] Vn=new float[4];
  Vn[0] = V1[1]*V2[2] - V2[1]*V1[2];
  Vn[1] = V2[0]*V1[2] - V1[0]*V2[2];
  Vn[2] = V1[0]*V2[1] - V2[0]*V1[1];
  Vn[3] = 1;
  m = sqrt(DogaDotProduct(Vn,Vn));
  if (m==0) {
    println("**BE CAREFUL THE LINES ARE PARALLEL TO EACH OTHER***");
    for (int i=0;i<3;i++) Vn[i] = 0;
  }
  else for (int i=0;i<3;i++) Vn[i] = Vn[i]/m;
  return Vn;
}
float DogaCrossProduct(float V1[],float V2[],float ApplySolution[]) {
  float m;
  float[] Vn=new float[4];
  Vn[0] = V1[1]*V2[2] - V2[1]*V1[2];
  Vn[1] = V2[0]*V1[2] - V1[0]*V2[2];
  Vn[2] = V1[0]*V2[1] - V2[0]*V1[1];
  Vn[3] = 1;
  m = sqrt(DogaDotProduct(Vn,Vn));
  if (m==0) {
    println("**BE CAREFUL THE LINES ARE PARALLEL TO EACH OTHER***");
    for (int i=0;i<3;i++) ApplySolution[i] = 0;
  }
  else for (int i=0;i<3;i++) ApplySolution[i] = Vn[i]/m;
  ApplySolution[3]=1;
  return m;
}
float DogaMakeUnitVector(float V[]) {
  float w=0;
  w = sqrt(DogaDotProduct(V,V));
  if (w!=0) {
    V[0] = V[0]/w;
    V[1] = V[1]/w;
    V[2] = V[2]/w;
  }
  else {
    V[0] = 0;
    V[1] = 0;
    V[2] = 0;
  }
  return w;
}
float[] DogaReturnUnitVector(float V[]) {
  float w=0,retV[]=new float[4];
  w = sqrt(DogaDotProduct(V,V));
  if (w!=0) {
    retV[0] = V[0]/w;
    retV[1] = V[1]/w;
    retV[2] = V[2]/w;
  }
  else {
    retV[0] = 0;
    retV[1] = 0;
    retV[2] = 0;
  }
  retV[3] = 1;
  return retV;
}
void DogaLoadIdentity(float[][] Matrix,int row,int col) {
  int i=0,j=0;
  for (i=0;i<row;i++)
    for (j=0;j<col;j++)
      if (i==j) Matrix[i][j]=1; else Matrix [i][j]=0;
}

float[][] DogaMatrixMultCol(float[][] matA,float[][] matB,int A,int B,int C) {
  float sol[][]=new float[A][C];
  int i,j,k;
  for (i=0;i<A;i++) {
    for (j=0;j<C;j++) {
      sol[i][j] = 0;
      for (k=0;k<B;k++)
  	sol[i][j] += matA[i][k]*matB[k][j];
    }
  }
  return sol;
}

void DogaMatrixMultCol(float[][] matA,float[][] matB,int A,int B,int C,float[][] ApplySolution) {
  float sol[][]=new float[A][C];
  int i,j,k;
  for (i=0;i<A;i++) {
    for (j=0;j<C;j++) {
      sol[i][j] = 0;
      for (k=0;k<B;k++)
  	sol[i][j] += matA[i][k]*matB[k][j];
    }
  }
  for (i=0;i<A;i++)
    for (j=0;j<C;j++)
       ApplySolution[i][j]=sol[i][j]; 
  return;
}
float[][] DogaMatrixMultRow(float[][] matA,float matB[][],int A,int B,int C) {
  float sol[][]=new float[A][C];
  int i,j,k;
  // matA will be AxB matrix ; matB will be BxC matrix ; solution will be AxC matrix
  for (i=0;i<A;i++)
    for (j=0;j<C;j++)
      sol[i][j] = 0;
  for (i=0;i<A;i++) {//A rows(points) of matA
    for (j=0;j<B;j++) {//B coloumns of matA and rows of matB
      for (k=0;k<C;k++) //C cols of matB
	sol[i][j] += matA[i][k]*matB[j][k];
    }
  }
  return sol;
}
void DogaMatrixMultRow(float[][] matA,float matB[][],int A,int B,int C,float[][] ApplySolution) {
  float sol[][]=new float[A][C];
  int i,j,k;
  // matA will be AxB matrix ; matB will be BxC matrix ; solution will be AxC matrix
  for (i=0;i<A;i++)
    for (j=0;j<C;j++)
      sol[i][j] = 0;
  for (i=0;i<A;i++) {//A rows(points) of matA
    for (j=0;j<B;j++) {//B coloumns of matA and rows of matB
      for (k=0;k<C;k++) //C cols of matB
	sol[i][j] += matA[i][k]*matB[j][k];
    }
  }
  for (i=0;i<A;i++)
   for (j=0;j<C;j++)
     ApplySolution[i][j] = sol[i][j];
  return;
}
/***************************************************************/
/*********************** GLOBAL VARIABLES***********************/
/***************************************************************/
int maxNumOfRootChains=8,maxNumOfChains=10;
int funcCCD=0,funcCIR=1;
int TRACKER=0,CLICKER=1,ARBITRARY=2;
int CIRCLEmove=0,SQUAREmove=1,RANDOMmove=2;

int mode=TRACKER,usedFunction=funcCCD;
int moveMode=CIRCLEmove,moveCnt=0;

int screenW=800,screenH=800;
float mouseSensitivity=0,minSensitivity=0,maxSensitivity=1500;
int myMouseX=0,myMouseY=0;

float P[][][] = new float[maxNumOfRootChains][maxNumOfChains][4];
float L[][] = new float[maxNumOfRootChains][maxNumOfChains];
float A[][] = new float[maxNumOfRootChains][maxNumOfChains];
float D1[]= new float[]{screenW/2,screenH/3,-10,1};

int numOfRootChains=1;
float rootsDif=20;
float totalLength[]=new float[maxNumOfRootChains],maxChainLength,minChainLength,chainLengthDif;
int nChain[]=new int[maxNumOfRootChains],minChain=2;
int cnt[]=new int[maxNumOfRootChains],maxCnt=10;

boolean rootSelect=false,chainSelect=false,amountSelect=false;
int selectedRoot=0,selectedChain=1,amount=0;

void randomMoveFunc() {
  float degree=(mouseSensitivity/300),X,Y;
  moveCnt=(moveCnt+1)%360;
  if (moveMode==CIRCLEmove) {
    if (moveCnt%50!=0) return;
    X = D1[0]-screenW/2;
    Y = D1[1]-screenH/2;
    if (degree==0) degree=1;
    D1[0] = X*cosinus(degree) - Y*sinus(degree) + screenW/2;
    D1[1] = X*sinus(degree) + Y*cosinus(degree) + screenH/2;
    for (int i=0;i<numOfRootChains;i++) cnt[i]=0;
  }
  if (moveMode==SQUAREmove) return;
  if (moveMode==RANDOMmove) return;
}

void cirIterate(int Pindex,float[] lineLength) {
  int i=0;
  float V1[]=new float[]{-P[Pindex][0][0]+D1[0],-P[Pindex][0][1]+D1[1],0};
  float midPoint[]=new float[]{(P[Pindex][0][0]+D1[0])/2,(P[Pindex][0][1]+D1[1])/2,0};
  float V2[]=new float[]{1,0,0};
  float V3[],V4[]=new float[4],Length,degree,Vk[]=new float[3],len,xLen;
  Length = sqrt(DogaDotProduct(V1,V1));
  lineLength[0]=Length;
  V3 = DogaReturnUnitVector(V1);
  DogaCrossProduct(V2,V3,V4);
  degree = rad2deg(acos(DogaDotProduct(V3,V2)));
  if (V4[2]<0) degree=360-degree;
  for (i=0;i<=nChain[Pindex];i++) A[Pindex][i]=degree;
  V4[0]=0; V4[1]=0; V4[2]=0; V4[3]=0;
  if (Length<=totalLength[Pindex]) {
    xLen = Length/nChain[Pindex];
    DogaCopyMatrix(P[Pindex][0],Vk,3);
    for (i=1;i<=nChain[Pindex];i++) {
      Vk[0] += xLen*V3[0];
      Vk[1] += xLen*V3[1];
      V4[0] = Vk[0] - midPoint[0];
      V4[1] = Vk[1] - midPoint[1];
      len = sqrt(DogaDotProduct(V4,V4));
      P[Pindex][i][1] = Vk[1];
      P[Pindex][i][0] = Vk[0]-sqrt(Length*Length*0.25-len*len)*((Length-totalLength[Pindex])/totalLength[Pindex]);
      P[Pindex][i][2] = -10;
    }
  }
}
float ccdIterate(float[] center,float[] end) {
  float V1[]=new float[]{-center[0]+D1[0],-center[1]+D1[1],0,1};
  float V2[]=new float[]{-center[0]+end[0],-center[1]+end[1],0,1};
  float V3[],V4[],V5[]=new float[4],degree;
  V3 = DogaReturnUnitVector(V1);
  V4 = DogaReturnUnitVector(V2);
  DogaCrossProduct(V1,V2,V5);
  degree = rad2deg(acos(DogaDotProduct(V3,V4)));
  degree = int(degree)%360;
  if (V5[2]>0) degree*=-1;
  return degree;
}
boolean done(float P1,float P2,float dif) {
  if (absD(P1-P2)<dif) return true;
  return false;
}
void drawMyIK() {
  int i=0,j=0;
  for (i=0;i<numOfRootChains;i++) {
    stroke(1,1,1);
    noFill();
    ellipseMode(CENTER);
    ellipse(P[i][0][0],height-P[i][0][1],totalLength[i]*2,totalLength[i]*2);
    stroke(255,255,255);
    fill(255,255,0);
    for (j=1;j<=nChain[i];j++) {
      line(P[i][j-1][0],height-P[i][j-1][1],P[i][j][0],height-P[i][j][1]);
      ellipse(P[i][j-1][0],height-P[i][j-1][1],10,10);
    }
    ellipse(P[i][j-1][0],height-P[i][j-1][1],10,10);
  }
}
void myIK() {
  int i=0,j=0,k=0;
  float lineLength[]=new float[1];
  for (i=0;i<numOfRootChains;i++) {
    if ((cnt[i]<=maxCnt)&&(usedFunction==funcCCD))
      for (j=nChain[i];j>0;j--) {
        //FIND THE ALFA VALUES FOR EACH ONE AT FIRST
        if (done(D1[0],P[i][nChain[i]][0],0.4)&&done(D1[1],P[i][nChain[i]][1],0.4)) cnt[i]=maxCnt+1;
        if (cnt[i]<=maxCnt) {
          A[i][j] += ccdIterate(P[i][j-1],P[i][nChain[i]]);
          //ITERATIVELY FIND ALL THE UPDATED POINTS
          for(k=1;k<=nChain[i];k++){
            P[i][k][0] = P[i][k-1][0] + L[i][k]*cosinus(A[i][k]);
            P[i][k][1] = P[i][k-1][1] + L[i][k]*sinus(A[i][k]);
            P[i][k][2] = -10;
          }
        }
      }
    else if ((cnt[i]<=maxCnt)&&(usedFunction==funcCIR)){
      cirIterate(i,lineLength);
      //ITERATIVELY FIND ALL THE UPDATED POINTS
      if (lineLength[0]>totalLength[i]) 
        for(k=1;k<=nChain[i];k++){
          P[i][k][0] = P[i][k-1][0] + L[i][k]*cosinus(A[i][k]);
          P[i][k][1] = P[i][k-1][1] + L[i][k]*sinus(A[i][k]);
          P[i][k][2] = -10;
        }
    }
    cnt[i]++;
  }
}

/***************************************************************/
/********* STANDART  PROCESSING  FIRST&LOOP FUNCTIONS***********/
/***************************************************************/
void setup() {
 int i=0,j=0;
 size(screenW,screenH);
 resetValues(true);
}
void draw() {
  int i=0;
  float dif=0;
  background(0,50,100);
  myMouseX=mouseX;
  myMouseY=height-mouseY;
  if (mode==TRACKER) {
    dif = absD((D1[0]-myMouseX)*(D1[0]-myMouseX)+(D1[1]-myMouseY)*(D1[1]-myMouseY));
    if (dif>mouseSensitivity) {
      D1[0] = myMouseX;
      D1[1] = myMouseY;
      for (i=0;i<numOfRootChains;i++)	
        cnt[i]=0;
    }
  }
  else if (mode==ARBITRARY) randomMoveFunc();
  myIK();
  drawMyIK();
  stroke(255,255,255);
  fill(255,0,0);
  ellipseMode(CENTER);
  ellipse(D1[0],height-D1[1],10,10);
}

void mousePressed(){
  float dif=0;
  int i=0;
  myMouseX=mouseX;
  myMouseY=height-mouseY;
  dif = absD((D1[0]-myMouseX)*(D1[0]-myMouseX)+(D1[1]-myMouseY)*(D1[1]-myMouseY));
  if ((dif>mouseSensitivity)&&(mode==CLICKER)) {
    D1[0] = myMouseX;
    D1[1] = myMouseY;
    for (i=0;i<numOfRootChains;i++)	
      cnt[i]=0;
  }
}

void keyPressed(){
  int num=0;
  if ((key>='0')&&(key<='9')) {
    num=key-'0';
    if (rootSelect) {
      selectedRoot=num;
      println("SelectedRoot= "+selectedRoot);
    }
    if (chainSelect) {
      selectedChain=num;
      println("SelectedChain="+selectedChain);
    }
    if (amountSelect) {
      amount=amount*10+num;
      println("Amount="+amount);
    }
  }
  else amount=0;
  if ((key=='q')||(key=='Q')) {
    rootSelect=true;
    chainSelect=false;
    amountSelect=false;
  }
  else if ((key=='w')||(key=='W')) {
    rootSelect=false;
    chainSelect=true;
    amountSelect=false;
  }
  else if ((key=='e')||(key=='E')) {
    rootSelect=false;
    chainSelect=false;
    amountSelect=true;
  }
  else if (key=='R') {//add one root
    if (numOfRootChains<maxNumOfRootChains) {
      numOfRootChains++;
      resetValues(true);
    }
  }
  else if (key=='r') {//delete one root
    if (numOfRootChains>1) {
      numOfRootChains--;
      resetValues(true);
    }
  }
  else if (key=='J') {//add one joint
    if (nChain[0]<maxNumOfChains-1) {
      for (int i=0;i<numOfRootChains;i++)
	nChain[i]++;
      resetValues(false);
    }
  }
  else if (key=='j') {//delete one root
    if (nChain[0]>minChain) {
      for (int i=0;i<numOfRootChains;i++)
	nChain[i]--;
      resetValues(false);
    }
  }
}

void resetValues(boolean rootChainReset) {
  int i=0,j=0;
  float deg;
  if (rootChainReset) {
    if (numOfRootChains<=4) {
      maxChainLength = screenW/2;
      minChainLength = screenW/4;
    }
    else {
      maxChainLength = screenW/4;
      minChainLength = screenW/8;
    }
    chainLengthDif = screenW/8;
    for (i=0;i<numOfRootChains;i++) {
      if (rootChainReset) totalLength[i] = minChainLength;
      if (rootChainReset) nChain[i] = (maxNumOfChains-numOfRootChains)/2+1;
    }
  }
  if (numOfRootChains==1) {
    P[0][0][0] = screenW/2;
    P[0][0][1] = screenH/2;
    P[0][0][2] = -10;
    P[0][0][3] = 1;
    for (j=0;j<=nChain[0];j++) A[0][j]=270;
  }
  else {
    deg = 360/numOfRootChains;
    for (i=0;i<numOfRootChains;i++) {
      P[i][0][0] = screenW/2+(cosinus(i*deg))*(0.5*screenW-(numOfRootChains-2)*rootsDif);
      P[i][0][1] = screenH/2+(sinus(i*deg))*(0.5*screenH-(numOfRootChains-2)*rootsDif);
      P[i][0][2] = -10;
      P[i][0][3] = 1;
      for (j=0;j<=nChain[i];j++) A[i][j]=0;
    }
  }
  for (i=0;i<numOfRootChains;i++) {
    L[i][0] = totalLength[i]/nChain[i];
    for (j=1;j<=nChain[i];j++) {
      L[i][j] = totalLength[i]/nChain[i];
      P[i][j][0] = P[i][j-1][0] + L[i][j]*cosinus(A[i][j]);
      P[i][j][1] = P[i][j-1][1] + L[i][j]*sinus(A[i][j]);
      P[i][j][2] = -10;
      P[i][j][3] = 1;
    }
    cnt[i]=0;
  }
}
